#include "sensor.h"
#include <QDebug>
#include <QTime>

Sensor::Sensor()
{
    SensorP=imread("E:/QT_corder/SmartCar_fundenment/chepter6/P1000C1.bmp",0);
    for(int i=0;i<720;i++)
    {
        theta[i]=i*0.5;

    }
    k[0]=-1000000;
    k[360]=-k[0];
    for(int i=1;i<360;i++)
    {
        k[i]=tan(double(i)/360*CV_PI+CV_PI/2);

        k[i+360]=k[i];
    }
   /* for(int i=0;i<720;i++)
    {
        qDebug()<<i/2<<k[i];
    }*/

    headAng=0;
    QTime timer1=QTime::currentTime();
    Scalar color;
    double dist;
    for(int i=0;i<720;i++)
    {
        dist=Dist(100,100,i/2+headAng);
 //       qDebug()<<headAng<<i/2<<dist;
    }
    int a=QTime::currentTime().msecsTo(timer1);
 //   qDebug()<<a;
}

void Sensor::SetInitPos(double x, double y,double heading)
{
    px=x;
    py=y;
    headAng=heading;


}

void Sensor::updateSensor(double x,double y,double heading,QVector<QPointF> *pt)
{
    double dist;
    for(int i=0;i<720;i++)
    {
        dist=Dist(x,y,double(i)/2+heading);
       // (*pt)[i].setX(double(i)/2);
        (*pt)[i].setY(dist);

      //  qDebug()<<headAng<<double(i)/2<<dist;
    }
}


double Sensor::Dist(double x0,double y0,double angle)
{
    int x1,y1;
    if(angle>360) angle=angle-360;
    if(angle<=0.001||angle>=359.999)
    {
        for(int i=1;i<2000;i++)
        {
            x1=int(x0);
            y1=int(y0+i);
            if(y1>999)
            {
                y1=999;
                break;
            }
            if(SensorP.ptr<uchar>(y1)[x1]<10)
            {
                break;
            }
        }
    }
    else if(angle<=180.001&&angle>=179.999)
    {
        for(int i=1;i<2000;i++)
        {
            x1=int(x0);
            y1=int(y0-i);
            if(y1<0)
            {
                y1=0;
                break;
            }
            if(SensorP.ptr<uchar>(y1)[x1]<10)
            {
                break;
            }
        }
    }
    else if(angle>=89.999 && angle<=90.001)
    {
        for(int i=1;i<2000;i++)
        {
            x1=int(x0-i);
            y1=int(y0);
            if (x1<=0)
            {
                x1=0;
                break;
            }
            if(SensorP.ptr<uchar>(y1)[x1]<10)
            {
                break;
            }
        }
    }
    else if(angle>=269.999 && angle<=270.001)
    {
        for(int i=1;i<2000;i++)
        {
            x1=int(x0+i);
            y1=int(y0);
            if (x1>999)
            {
                x1=999;
                break;
            }
            if(SensorP.ptr<uchar>(y1)[x1]<10)
            {
                break;
            }
        }
    }
    else if(angle <45||angle>315)
    {
        for(int i=1;i<2000;i++)
        {
            y1=int(y0+i);
            x1=int(x0-tan(angle/180*CV_PI)*(double(y1)-y0));
            if (y1>999)
            {
                y1=999;
                if(x1>=999)
                    x1=999;
                else if(x1<0)
                    x1=0;
                break;
            }
            else
            {
                if(x1>=999)
                {
                    x1=999;
                    break;
                }
                else if(x1<0)
                {
                    x1=0;
                    break;
                }
            }
            if(SensorP.ptr<uchar>(y1)[x1]<10)
            {
                break;
            }
        }
    }
    else if(angle >=45&&angle<135)
    {
        for(int i=1;i<2000;i++)
        {
            x1=double(x0-i);
            y1=int((x0-double(x1))/tan(angle/180*CV_PI)+y0);
            if (x1<0)
            {
                x1=0;
                if(y1>=999)
                    y1=999;
                else if(y1<0)
                    y1=0;
                break;
            }
            else
            {
                if(y1>=999)
                {
                    y1=999;
                    break;
                }
                else if(y1<0)
                {
                    y1=0;
                    break;
                }
            }
            if(SensorP.ptr<uchar>(y1)[x1]<10)
            {
                break;
            }
        }
    }
    else if(angle >=135&&angle<225)
    {
        for(int i=1;i<2000;i++)
        {
            y1=int(y0-i);
            x1=int(x0-tan(angle/180*CV_PI)*(double(y1)-y0));
            if (y1<0)
            {
                y1=0;
                if(x1>=999)
                    x1=999;
                else if(x1<0)
                    x1=0;
                break;
            }
            else
            {
                if(x1>=999)
                {
                    x1=999;
                    break;
                }
                else if(x1<0)
                {
                    x1=0;
                    break;
                }
            }
            if(SensorP.ptr<uchar>(y1)[x1]<10)
            {
                break;
            }
        }
    }
    else if(angle >=225&&angle<=315)
    {
        for(int i=1;i<2000;i++)
        {
            x1=int(x0+i);
            y1=int((x0-double(x1))/tan(angle/180*CV_PI)+y0);
            if (x1>999)
            {
                x1=999;
                if(y1>=999)
                    y1=999;
                else if(y1<0)
                    y1=0;
                break;
            }
            else
            {
                if(y1>=999)
                {
                    y1=999;
                    break;
                }
                else if(y1<0)
                {
                    y1=0;
                    break;
                }
            }
            if(SensorP.ptr<uchar>(y1)[x1]<10)
            {
                break;
            }
        }
    }
    //qsrand(QTime(0,0,0).secsTo(QTime::currentTime()));
    //qsrand(time(NULL));

    return sqrt((x0-double(x1))*(x0-double(x1))+(y0-double(y1))*(y0-double(y1)));//+qrand()%10*0.9;
}
